#include <tf2/LinearMath/Quaternion.h>

#include <rclcpp/logging.hpp>
#include <rclcpp/qos.hpp>
#include <rclcpp/utilities.hpp>
#include <serial_driver/serial_driver.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

// C++ system
#include <cstdint>
#include <functional>
#include <map>
#include <memory>
#include <string>
#include <vector>

#include "mas_serial_driver_second/mas_serial_driver.hpp"
#include "rm_utils/math/utils.hpp"
#include "rm_interfaces/msg/gimbal_cmd.hpp"
#include "rm_interfaces/msg/serial_receive_data.hpp"
#include "rm_interfaces/srv/set_mode.hpp"
#include "rm_interfaces/msg/gimbal_cmd.hpp"

namespace mas_serial_driver_second
{
RMSerialDriver::RMSerialDriver(const rclcpp::NodeOptions & options)
: Node("mas_serial_driver_second", options),
  owned_ctx_{new IoContext(2)},
  serial_driver_{new drivers::serial_driver::SerialDriver(*owned_ctx_)}
{
  RCLCPP_INFO(get_logger(), "Start RMSerialDriver!");

  getParams();

  target_frame_ = this->declare_parameter("target_frame", "odom");

  // TF broadcaster
  timestamp_offset_ = this->declare_parameter("timestamp_offset", 0.0);
  tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);

  // Publisher
  serial_receive_data_pub_ = this->create_publisher<rm_interfaces::msg::SerialReceiveData>("serial/receive", rclcpp::SensorDataQoS());
  // Subscriber
  gimbal_cmd_sub_ = this->create_subscription<rm_interfaces::msg::GimbalCmd>("armor_solver/cmd_gimbal",rclcpp::SensorDataQoS(),std::bind(&RMSerialDriver::sendData, this, std::placeholders::_1));


  // Service clients
  set_mode_clients_.push_back(this->create_client<rm_interfaces::srv::SetMode>("/second/armor_detector/set_mode", rmw_qos_profile_services_default));
  set_mode_clients_.push_back(this->create_client<rm_interfaces::srv::SetMode>("/second/armor_solver/set_mode", rmw_qos_profile_services_default));

  try {
    serial_driver_->init_port(device_name_, *device_config_);
    if (!serial_driver_->port()->is_open()) {
      serial_driver_->port()->open();
      receive_thread_ = std::thread(&RMSerialDriver::receiveData, this);
    }
  } catch (const std::exception & ex) {
    RCLCPP_ERROR(
      get_logger(), "Error creating serial port: %s - %s", device_name_.c_str(), ex.what());
    throw ex;
  }

}

RMSerialDriver::~RMSerialDriver()
{
  if (receive_thread_.joinable()) {
    receive_thread_.join();
  }

  if (serial_driver_->port()->is_open()) {
    serial_driver_->port()->close();
  }

  if (owned_ctx_) {
    owned_ctx_->waitForExit();
  }
}

void RMSerialDriver::receiveData()
{
  std::vector<uint8_t> header(1);
  std::vector<uint8_t> data;
  data.reserve(sizeof(ReceivePacket)); 

  while (rclcpp::ok()) {
    try {
      serial_driver_->port()->receive(header);
      if (header[0] == 0x5A) {
        // Read the rest of the message
        data.resize(sizeof(ReceivePacket)-1); 
        serial_driver_->port()->receive(data);
        data.insert(data.begin(), header[0]);
        ReceivePacket packet = fromVector(data);

        if (packet.tail == 0x5D) 
        {
          // Parse the message
          rm_interfaces::msg::SerialReceiveData msgpacket;
          msgpacket.header.stamp = this->now() + rclcpp::Duration::from_seconds(timestamp_offset_);
          msgpacket.header.frame_id = target_frame_;
          msgpacket.mode = packet.mode;
          msgpacket.roll = packet.roll;
          msgpacket.yaw = packet.yaw;
          msgpacket.pitch = packet.pitch;
          serial_receive_data_pub_->publish(msgpacket);


          // Set mode only if it is different from the current mode
          if (packet.mode != current_mode_) {
            setMode(packet.mode);
            current_mode_ = packet.mode; // Update current mode
          }

          geometry_msgs::msg::TransformStamped t;
          timestamp_offset_ = this->get_parameter("timestamp_offset").as_double();
          t.header.stamp = this->now() + rclcpp::Duration::from_seconds(timestamp_offset_);
          t.header.frame_id = target_frame_;
          t.child_frame_id = "gimbal_link_second";
          auto roll = packet.roll * M_PI / 180.0;
          auto pitch = -packet.pitch * M_PI / 180.0;
          auto yaw = packet.yaw * M_PI / 180.0;
          tf2::Quaternion q;
          q.setRPY(roll, pitch, yaw);
          t.transform.rotation = tf2::toMsg(q);
          tf_broadcaster_->sendTransform(t);

          // odom_rectify: 转了roll角后的坐标系
          Eigen::Quaterniond q_eigen(q.w(), q.x(), q.y(), q.z());
          Eigen::Vector3d rpy = fyt::utils::getRPY(q_eigen.toRotationMatrix());
          q.setRPY(rpy[0], 0, 0);
          t.header.frame_id = target_frame_;
          t.child_frame_id = target_frame_ +"_rectify";

          tf_broadcaster_->sendTransform(t);
        }else {
          RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 20, "Invalid tail: %02X", packet.tail);
        }
      } else {
        RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 20, "Invalid header: %02X", header[0]);
      }
    } catch (const std::exception & ex) {
      RCLCPP_ERROR_THROTTLE(
        get_logger(), *get_clock(), 20, "Error while receiving data: %s", ex.what());
      reopenPort();
    }
  }
}

void RMSerialDriver::sendData(const rm_interfaces::msg::GimbalCmd::SharedPtr msg)
{
  try {
    SendPacket packet;
    packet.header = 0x5A;
    packet.distance = msg->distance;
    packet.fire_advice = msg->fire_advice;
    packet.pitch = msg->pitch;
    packet.yaw = msg->yaw;
    packet.tail = 0x5D;

    std::vector<uint8_t> data = toVector(packet);

    serial_driver_->port()->send(data);

  } catch (const std::exception & ex) {
    RCLCPP_ERROR(get_logger(), "Error while sending data: %s", ex.what());
    reopenPort();
  }
}

void RMSerialDriver::getParams()
{
  using FlowControl = drivers::serial_driver::FlowControl;
  using Parity = drivers::serial_driver::Parity;
  using StopBits = drivers::serial_driver::StopBits;

  uint32_t baud_rate{};
  auto fc = FlowControl::NONE;
  auto pt = Parity::NONE;
  auto sb = StopBits::ONE;

  try {
    device_name_ = declare_parameter<std::string>("device_name", "");
  } catch (rclcpp::ParameterTypeException & ex) {
    RCLCPP_ERROR(get_logger(), "The device name provided was invalid");
    throw ex;
  }

  try {
    baud_rate = declare_parameter<int>("baud_rate", 0);
  } catch (rclcpp::ParameterTypeException & ex) {
    RCLCPP_ERROR(get_logger(), "The baud_rate provided was invalid");
    throw ex;
  }

  try {
    const auto fc_string = declare_parameter<std::string>("flow_control", "");

    if (fc_string == "none") {
      fc = FlowControl::NONE;
    } else if (fc_string == "hardware") {
      fc = FlowControl::HARDWARE;
    } else if (fc_string == "software") {
      fc = FlowControl::SOFTWARE;
    } else {
      throw std::invalid_argument{
        "The flow_control parameter must be one of: none, software, or hardware."};
    }
  } catch (rclcpp::ParameterTypeException & ex) {
    RCLCPP_ERROR(get_logger(), "The flow_control provided was invalid");
    throw ex;
  }

  try {
    const auto pt_string = declare_parameter<std::string>("parity", "");

    if (pt_string == "none") {
      pt = Parity::NONE;
    } else if (pt_string == "odd") {
      pt = Parity::ODD;
    } else if (pt_string == "even") {
      pt = Parity::EVEN;
    } else {
      throw std::invalid_argument{"The parity parameter must be one of: none, odd, or even."};
    }
  } catch (rclcpp::ParameterTypeException & ex) {
    RCLCPP_ERROR(get_logger(), "The parity provided was invalid");
    throw ex;
  }

  try {
    const auto sb_string = declare_parameter<std::string>("stop_bits", "");

    if (sb_string == "1" || sb_string == "1.0") {
      sb = StopBits::ONE;
    } else if (sb_string == "1.5") {
      sb = StopBits::ONE_POINT_FIVE;
    } else if (sb_string == "2" || sb_string == "2.0") {
      sb = StopBits::TWO;
    } else {
      throw std::invalid_argument{"The stop_bits parameter must be one of: 1, 1.5, or 2."};
    }
  } catch (rclcpp::ParameterTypeException & ex) {
    RCLCPP_ERROR(get_logger(), "The stop_bits provided was invalid");
    throw ex;
  }

  device_config_ =
    std::make_unique<drivers::serial_driver::SerialPortConfig>(baud_rate, fc, pt, sb);
}

void RMSerialDriver::reopenPort()
{
  RCLCPP_WARN(get_logger(), "Attempting to reopen port");
  try {
    if (serial_driver_->port()->is_open()) {
      serial_driver_->port()->close();
    }
    serial_driver_->port()->open();
    RCLCPP_INFO(get_logger(), "Successfully reopened port");
  } catch (const std::exception & ex) {
    RCLCPP_ERROR(get_logger(), "Error while reopening port: %s", ex.what());
    if (rclcpp::ok()) {
      rclcpp::sleep_for(std::chrono::seconds(1));
      reopenPort();
    }
  }
}

void RMSerialDriver::setMode(const uint8_t mode)
{
    using namespace std::chrono_literals;

    for (auto & client : set_mode_clients_) {
            RCLCPP_INFO(get_logger(), "Attempting to set mode %d for service %s", mode, client->get_service_name());
            // Wait for service
            while (!client->wait_for_service(1s)) {
                if (!rclcpp::ok()) {
                    RCLCPP_ERROR(get_logger(), "Interrupted while waiting for the service {%s}. Exiting.", client->get_service_name());
                    return;
                }
                RCLCPP_INFO(get_logger(), "Service {%s} not available, waiting again...", client->get_service_name());
            }

            if (!client->service_is_ready()) {
                RCLCPP_WARN(get_logger(), "Service: {%s} is not available!", client->get_service_name());
                return;
            }

            // Send request
            auto req = std::make_shared<rm_interfaces::srv::SetMode::Request>();
            req->mode = mode;

            RCLCPP_INFO(get_logger(), "Sending request to set mode %d for service %s", mode, client->get_service_name());

            // Use a callback to handle the response
            auto response_received_callback = [this, mode, service_name = client->get_service_name()](const rclcpp::Client<rm_interfaces::srv::SetMode>::SharedFuture future) {
                try {
                    auto result = future.get();
                    if (result->success) {
                        RCLCPP_INFO(get_logger(), "Mode set to %d for service %s", mode, service_name);
                    } else {
                        RCLCPP_WARN(get_logger(), "Failed to set mode for service %s", service_name);
                    }
                } catch (const std::exception & ex) {
                    RCLCPP_ERROR(get_logger(), "Exception while handling response for service %s: %s", service_name, ex.what());
                }
            };

            client->async_send_request(req, response_received_callback);
    }
}

}  

#include "rclcpp_components/register_node_macro.hpp"

// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(mas_serial_driver_second::RMSerialDriver)